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Dynamic inversion has often been used in the simulation environment to rapidly prototype controls for the 
full flight envelope, because of its capacity for assessing a vehicle’s maneuver performance and proper sizing 
of control surfaces. Generally, the architectures involve either a direct inversion of the entire set of equations 
of motion or a sequential set of inversions exploiting time scale separation in the vehicle dynamics where 
faster parameters are considered as controls for slower varying parameters. The proposed architecture 
builds on the latter using a quaternion formulation that provides singularity free tracking. Of interest, the 
proposed architecture simplifies the sequential approach by exploiting a simpler kinematic inversion in place 
of a more difficult inversion typically used. This kinematic relationship accurately describes the angular rate 
required to drive some reference frame of interest to a desired attitude at some desired quaternion error rate. 
A simple PID control is used to define the desired quaternion error rate. The paper develops the theoretical 
framework for the approach, and shows results in tracking a desired trajectory. 


I. Introduction 

A recently developed quaternion-based control for a notional launch abort system is reformulated as an analysis tool 
to address aircraft safety applications in assessing recovery maneuver control power requirements, providing a 
maneuvering envelop based on control power limitations of the damaged vehicle, and characterizing reachable 
response dynamics for control adaptation. The quaternion formulation provides a concise singularity-free 
description of the orientation of one reference frame to another. For crew escape, the control provided singularity - 
free tracking of the vehicle with respect to the wind axis system (defined with the angle of attack and side-slip 
angle) with coordinated moment commands from pitch-over and coast-to-reorientation and heat- shield-for ward 
flight. For aircraft, the extended approach presented in this paper includes control of wind axis bank angle along 
with angle-of-attack and sideslip angle — all three angles are drivers in determining the orientation of the vehicle’s 
lift vector and consequently the movement of the vehicle’s inertial velocity vector (i.e. maneuvering). In both cases 
the control exploits the two-timescale nature of the controlled dynamics. Here, the slower loop, utilizing a simple 
quaternion relationship, controls the orientation of the vehicle with respect to, not the wind-axis, but the A-frame 
(separated from the wind-axis by the wind-axis bank angle and the inertial axis by heading and flight path angle), 
and generates a set of commanded angular body rates. The angular rate commands are followed by a faster 
dynamic-inversion-based inner loop control that determines the required vehicle’s control moments. To follow 
candidate trajectories, an outer guidance loop, based on inverted point mass equations, is added to generate the 
necessary angle of attack, side-slip angle, and wind-axis bank angle commands along with required throttle 
commands. A control allocation algorithm will determine whether the required control moments can be realized or 
whether the candidate recovery trajectory needs to be modified. 

At this point in the tool’s development, however, no candidate recovery trajectories have been modified or 
envelopes of acceptable maneuvering have been derived. In this paper, the control architecture is described and 
implemented in the simulation of a subscale vehicle. An illustrative example of its potential for following a desired 
trajectory is presented. It should be mentioned that there are many options available both in academia and in 
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industry regarding control architectures capable for rapidly prototyping full -flight-envelope controls, determining 
vehicle maneuver performance and in the selection of control surface sizing. Most are based on dynamic inversion, 
involving direct manipulation of the equations of motions. Some use acceleration feedback, acceptable in the 
simulation environment — not necessarily acceptable in flight controls — to perform the inversion and realize lower- 
order responses across the flight envelop [1],[2],[3]. Others exploit the two-time-scale nature of the vehicle 
discussed above, and perform a sequential inversion treating faster-varying parameters as controls for slower 
parameters [4] . The proposed architecture builds on the latter, specifically the work of [4] . Interestingly, the closing 
remarks of [4] included a comment that a quaternion implementation could possibly overcome some of the 
singularities of the approach. It turns out that not only many of the singularities are eliminated, but quaternions can 
be used to eliminate one of the sequential inversions involving wind-axis bank angle, sideslip angle and angle of 
attack and replace it with a much simpler kinematic inversion producing a more accurate relationship between the 
desired attitude and the angular rate required to achieve it. 

To facilitate the discussion, the paper is organized as follows. First a tutorial is provided regarding quaternions, 
quaternion operations and properties. A simple quaternion-based PID control is then developed and demonstrated 
via an example with features that will be used in both the control and guidance portions of proposed architecture. 
Discussions follow concerning the outer-loop A-frame attitude control, the inner-loop body-rate controls, the control 
allocator algorithm, and finally, the guidance law. An illustrative example will follow demonstrating both the 
benefits and the areas needing more work. 

To avoid any confusion, the following nomenclature is adopted. Let denote the angular rate of frame B with 

respect to frame A expressed in the coordinates of frame C. Similarly, let jv/ denote the velocity of the origin of 

frame B with respect to frame A expressed in the coordinates of frame C. Moreover let j D Vg A } c denote the time 

derivative taken with respect to frame D of V B , the result expressed in coordinates of frame C. More nomenclature 
will be provided in the next section on quaternions. 


II. Quaternion Definition, Operations and Properties 

The quaternion defining the attitude of frame B with respect to frame A can be expressed as 
J cos(£ A2fi / 2) 

<?A2B _sin(£ A2B /2 ){n A2B } A 

an angle s A2B to orient itself to coincide with frame B. Since frame A rotates about {n A2B } A to obtain B , the 
coordinates of n A2B remain the same for both frames, i.e. {n A2B ) A = { n A2B ] B • For our purposes, the quaternions are 
all of unit length, q T A2B q A2B = 1 • 


where frame A undergoes a right-handed rotation about unit vector [n A2B } A through 


The relative attitude of one frame to another is often expressed by successive right-handed rotations about different 
axes using intermediate reference frames. Let q A2C and p C2B respectively designate quaternions defining the 
attitude of frame C with respect to frame A and the attitude of frame B with respect to frame C. Successive rotations 
can be accomplished with quaternion multiplication ( * ) defined as 


QaIB QaIC 


% 

J p ° ’ 


%Po-{<l}c-{p}c 


_M C _ 


>{Wc + ^oMc+W c x{Wc. 


( 1 ) 
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where q 0 = cos(s A2C / 2) , p 0 = cos(s C2B / 2) , {g} A = sin(£ A2C / 2) {rc A2C } A , { p) c = sin(> C2A / 2) {^z C2A } c and as noted 
previously, {g} A = {g} c . Note although the lower vector portion of the multiplication definition is in coordinates 
of the frame C, it still satisfies 


A2B } a 


(go{p} c + Po{g} c +{g} c x {p} c ) 
sin(>, 2 „ / 2) 


with identical coordinate values in frames A and B. A useful property satisfied by quaternion multiplication is the 
associative property, or 


Va2D = * PC2B * V B2D = <1a2C * (P C 2B * V B2 D ) = (Ba2C * P C 2B ) * V B2D • 

Quaternion multiplication, however, does not commute, i.e. q A2C * p C2B ^ p C2B * q A2C , except when one quaternion 
is the inverse of the other. 

The inverse of quaternion q A2C , defining the attitude of frame A with respect to frame C, is defined as 


^C2A ^A2C 



( 2 ) 


and satisfies q A2C *q A2C = q A2C *q A2C = [1 0 0 of. Of course, the two multiplications correspond to two 

different relative attitudes: the former is the attitude of frame A with itself and the latter is the attitude of frame C 
with itself. 


For control, there is one ambiguity in the quaternion description to be noted. Both q A2B and -q A2B are equivalent 
quaternions in that they provide the same relative attitude between the two frames. The negated one, however, 
utilizes a rotation in the opposite direction through a complementary angle s A2B ' satisfying £ A2fi - s A2B ' = ±2;r 

where |^ A2fi '| < 2 n . This ambiguity will be important in a quaternion-based control where one frame is to be driven 
to a commanded one using the shortest angular path possible. 

Consider now frame B rotating with an angular rate with respect to frame A, co B . The quaternion q A2B evolves 
according to [5] 


1 

4a2B=-<1a2B* 




( 3 ) 


Note that although the lower vector portion of q A2B has equivalent coordinates in frame A and B , the angular rate is 
expressed in coordinates of B, or {co B j , not as \co B j . Let us expand (3) into a matrix-vector multiplication using 

the definition of quaternion multiplication in (1) where q A2B - \q 0 q x q 2 q 3 f and j = \jd x co y co z J , so 
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q A2 B -~ 


( 4 ) 


q x q 2 q 3 

% ~<l3 ^2 

q 3 q 0 —q l 
~<h <h % 



1 

2 


Qi{^b} b - 


From (4), the derivative q A2B is always orthogonal to q A2B implying that the trajectory of q A2B moves on the 
surface of a 4-dimensional hyper sphere of unit radius. This restriction will be required of any desired q A2B that a 
control may be asked to realize. 


A last property to be considered is using quaternions to transform vector v expressed in coordinates of frame A to 
coordinates of frame B. Defining q A2B - \ ~ q 0 , the operation to obtain |v} g from |v} A is 




0 


0 

r ) 

= q A2B * 

f ) 

LWbJ 

LWaJ 


iq A2B= q A2B*{ V } A * q A2B 


( 5 ) 


This expression may be expanded with the definitions of quaternion multiplication and inverse to produce a simpler, 
equivalent result 


Ms = [ 2 (A + ( q 0 -(WM {q} A )I -2<?o {g} a ]Ma • 

The cross product matrix |g| is given by 



" 0 

-q 3 

q 2 

{e},= 

q 3 

0 



_-q 2 

<h 

0 


( 6 ) 


The matrix in (6) is equivalent to a direction cosine matrix[5]. 


III. Simple PID Quaternion Control Example 

At the heart of both the guidance and control loops in the proposed quaternion -based control is a PID controller that 
directly manipulates two quaternions representing the actual and desired attitudes of some key reference frame 
relative to a second reference frame. Due to the commonality of both loops, it is advantageous to look at a simple 
example that contains all the features/algorithms of the PID quaternion controller. To define the problem, suppose 
one wants to control the attitude of object B relative to frame A. Let frame B be fixed to the object. Let the attitude 
of frame C with respect to A be the desired attitude of the object. The actual and desired attitudes are defined by 
quaternions q A2B and q A2C respectively. Define the attitudes also by a successive angular rotation sequence 

— » 6 y , abbreviated as 1-3-2, (first about the v-axis in the A frame and then about subsequent z and y axes in 
the intermediate frames) and equivalently expressed with quaternion multiplication as 

q A2B=% x * q ez* c ley ( 7 ) 


where the intermediate quaternions have the form 
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9 fo = 

"cos(^/2)“ 

sm(0j2) 

0 

- 9 Sz = 

cos(# z /2) 

0 

0 

, and q 9y = 

cos(<9 y /2) 

0 

sin(6' v /2) 


0 


_sin(<9 z /2)_ 


0 


A similar set of angles (0 x c —> 6 z e —>0 y c ) specify the object’s desired attitude where q A2C — q Qx c *q 6z c *q 0y c • 
For simplicity, assume that a first-order system governs angular rate j of the object to some commanded 

angular rate response {co B } with a diagonal transfer matrix with elements — - — for k = x,y,z ■ From 

{co B | and q A2B , equation (3) governs the object’s resulting attitude q A2B . 

For control purposes, the attitude change required of the object to move to the desired attitude is 

Qe ~ 9 a2B * C Ia2C = Q B2C (^) 

The attitudes of frames B and C coincide when q e = [l 0 0 0] r . To develop an expression governing the motion 
of q e , note first that q e satisfies q A2B *q e = q A2C . From (1), it is easily shown (using an equivalent matrix-vector 
product expression) that the derivative operator distributes over quaternion multiplication, or 

=q A 2c ( 10 ) 

Substituting for q A2B and q A2C using (3), 

^ *{®s} B *? e + 9.428 * =^?A2C*{®c} c ( U ) 

Multiplying through by q~ A2B from the left, 

9 e =^i ( l~A2B* ( lA2C*{^c} c -{^B} B * ( le) ( 12 ) 

9. = ^(9 e *{®c} c -{®s} s *9 e )- (13) 

To drive q e to some desired q e des , this kinematic expression can be inverted to obtain the required commanded 
angular rate 

{<% )b_cm D -9, *{a> A c} c )*q;' (14) 

An equivalent simple matrix- vector expression can be obtained by noting q e *{oic\ c = Q eX {&>c } c anc * 

{«« }„, *9. =0,2 {««}„, where 
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( 15 ) 


“9, 1 -‘lei -‘lei 

9,0 9,3 -<7,2 

-9,3 9,0 9,1 

. 9,2 -9,i 9,0 


and Q el is defined in (4). Since the columns of Q e2 are orthonormal, (13) can be solved for { C 0 g\ as 

v ) B _ CMD 

-a, {<},> ■ (*«> 

To construct a corrective q e des , define the tracking error as 

A< 7 e = [l 0 0 Of -(q e ) (17) 

A 

where (q e ) denotes that a sign change is applied to q e to make the first entry q e0 > 0 . While it is true that 
quaternions q e and -q e correspond to the same relative attitude, using the one with q e0 < 0 will produce an error 
direction that will force the object to rotate the long way around to the target attitude. In the control, we will 
exclusively use this ‘aligned’ q e for all occurrences of q e (e.g., definitions of Q el and Q e2 ) up to the calculation of 

\co p . To simplify nomenclature, q e will denote (q e ) - 

A PID gain set will set the desired q e des as 

9 , _ dee = QelQel i k ,Me + K J Me dt +Kle^e ) (18) 

where the orthogonal projection Q eX Q T eX assures that q e des is directionally realizable since it is always orthogonal to 
q e , i.e. yi(Q el ) -L q e . From (13), the rate term can be expanded as A q e - —q e = - ^ (q e * * q e ) . 

The control is summarized in figure 1 and utilizes (16) and (17). The control is implemented digitally using a 
trapezoidal integration scheme for the integrated error. The three scalar gains in (18) can be selected using any 
PID gain optimizer, such as the one available in the MATLAB Control Design Toolbox. 

One controller input not discussed thus far is the angular rate of the commanded attitude {^c } c • Since this is 

usually not available, the input must be constructed from signal q A2C . To do this, care must be taken when the sign 
changes arbitrarily between the previous and current samples, q A2C (k-l) and q A2C (k ) respectively. The approach 
taken here is to first define the two possibilities: q M - ( q A2C ( k ) - q A2C ( k - 1)) / At or 

q p - (q A2C (k) + q A2C (k - 1)) / At where At is the sample time. Then, simply choose the estimate with the smallest 
inner product, e.g. if q M -q M <q P -q P , then q A2C - q M . Using the definition of Q x from (4) with the current 
sample q A2C (k) defining the matrix elements, the estimate of {^c } c can obtained as 

{cd*(k)} c =2Qf(k)4 A2c (k). (19) 
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K) c 



Figure 1. Quaternion-based PID control example 

Tracking results, with q A2B and q A2C decomposed into 1-3-2 Euler angles, are shown in figure 2 for two different 
choices of gain k de on A q e . The simple example presented in this section demonstrates the potential of a 
quaternion-based control for good angular tracking with the characteristics of a low-order response for each attitude 
angle. Regarding the Euler angles in figure 2, the reader is directed to [5] for a method of extracting the Euler 
angles from the terms of the corresponding direction cosine matrix which equals the matrix in equation (6). 

As stated, key features of the example will appear in both the guidance and the control loops of the proposed 
quaternion-based control architecture. Those features include 

1) constructing q e that defines the attitude change required to drive the actual to the desired frame, 

2) using a PID gain set to define a desired rate q e des to drive q e to [l 0 0 0] r representing perfect tracking, 
and 

3) using q e des in an inverse kinematic expression to define a desired angular rate of the actual frame 

B to move it towards the commanded frame C. 

Note this angular rate is expressed with respect to frame A. Interestingly, there is no restriction in the development 
thus far that frame A is stationary. Frame A could be rotating with respect to some other frame D and in fact, will 
be in the next section. 


IV. Quaternion-Based Control Architecture 

The architecture developed in this section reflects the notion that maneuvering flight is a matter of controlling the 
vehicle speed along with the lift vector’s magnitude and direction in the inertial frame (fixed earth). As many have 
done [4], the control exploits the two-time-scale nature of the controlled dynamics. Here, the slower loop, utilizing 
the simple quaternion relationship developed above, controls the orientation of the vehicle with respect to a slower 

moving frame N, generating a set of commanded body rates with respect to frame N, j co B . The attitude of 

frame 77 is defined by the direction of the vehicle’s velocity vector in the inertial frame /, i.e., heading and flight path 
angles. The attitude of the body frame B with respect to N is defined by three angles: wind-axis bank angle, sideslip 
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Figure 2. Tracking Results of Quaternion-Based PID Control 

angle, and angle of attack. The resulting commanded body rate j co N B is then combined with an estimate of the 
angular rate of N with respect to I (in body coordinates) to produce the command body rate with respect to /, or 

{(oL) ={tf£ } +«} . (20) 

1 B J B _ CMD 1 N )B ) B _ CMD V 7 

This angular rate command drives a faster dynamic -inversion-based inner-loop control that determines the required 
vehicle control moments. To follow candidate trajectories, an outer guidance loop, based on inverted point mass 
equations with inputs from a second quaternion based PID control, is added to generate the necessary angle of 
attack, side-slip, and wind-axis bank angle commands along with the required thrust commands. The overall 
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Figure 3. Quaternion-Based Control Architecture 

architecture is summarized in figure 3. Subject to limitations in control power, a control allocation algorithm is used 
to realize the desired control moments if possible. 

In the combined guidance and control system, the attitude of the body with respect to frame N and the attitude of 
frame N with respect to the inertial frame are expressed as quaternions satisfying 

c 1nir ( 21 ) 

< li2N~ < lx*^r ( 22 ) 

where //, /?, a, %, and / respectively denote the angles of wind axis bank, side-slip, angle of attack, heading and 
flight path. A 1-3-2 set of rotations defines q N2B where it is noted that a negative side-slip rotation is specified 
about the intermediate z axis. The sequence of successive rotations defining q I2N is 3-2. Generally, the vehicle’s 
equations of motion already yield the quaternion of the body with respect to the inertial frame, q I2B . This 
quaternion enables a more efficient calculation of q N2B , or 


QnIB ~ QllN I2B 


(23) 


where one quaternion multiplication operation is replaced by the simpler inverse operation (changing sign on the 
rotation vector part of q I2N ). 

Turning first to the control of q N2B , the quaternion representing the commanded attitude of the body with respect to 
N , q N2C , is assembled from the commanded wind-axis bank angle, side-slip angle, and angle of attack as 


9 

American Institute of Aeronautics and Astronautics 








Qn 2C — Qju c *V-P C *<la c 


(24) 


The commanded angles will be supplied by the guidance algorithm. The commanded angular rate (&)} c is 
obtained from (19). As in the example, the relative attitude of the command to the actual attitude is defined as 

Qe ~ Q N2B * Qn2C ~ $B2C (2^) 


with a tracking error expressed as 

Aft=[l 0 0 0 f-(q e ). (26) 

The aligned version of q e used above will replace q e in all subsequent calculations leading up to the generation of 
{ cd" } . These calculations include 

i 15 )B_CMD 


Ve.des = QeiQel ^A/, + K J MA +KuM e ) where 


(27) 


A e = -q e = *{"c } c -{®s } B *<i') ■ ( 28 ) 

Here, the required actual and commanded angular rates, j co B (&) and [A c {k)} c , are estimated using (19). The 

command [coE } is defined by the inverse kinematic relation (5), 

1 B )b_cmd j v 

WU — • -a, WL> <*» 


where the elements of q e define Q el and Q e2 as in (4) and (15). The equations of motion governing angular 
acceleration determine j from the external moments applied to the vehicle. Moments required to drive j 

to some commanded cmd will be determined by inverting these equations and replacing the angular 

acceleration j B cb B | with a desired one defined by a stabilizing inner-loop control. The commanded rate 
[o^ B cmd is defined as the sum of {co B and the slower-changing , corresponding to the angular rate 

of the vehicle’s velocity vector with respect to the inertial frame. Here, \co ! N (&)} can be estimated from the current 
and previous value of q I1N (k) using (19) and transformed to the appropriate coordinates via (5), or 

} g = Qn2b * K * Qn2B (30) 


where the over bar signifies \cd b } b = 0 • Technically, one could obtain as -{&>£ } 

| is from the equations of motion. This is not recommended, though, because j and \cd b | are not 
exactly in sync and their difference can produce undesirable transients not found in the estimated } g • The 
elements of the a- [5- ju ( q N2B ) outer-loop are summarized in figure 4. 


since 
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<iN2c = q Me * q- Pe * q« c <1 nib =<i m * <i-p * q a <in n = q z * % 

Figure 4. Quaternion-based outer loop control architecture 

The architecture of the quaternion-based control exploits the slow/fast time scales separating the vehicle’s responses 
of a , p, id and • Deployment of the vehicle’s effectors mainly create moments that are integrated to yield 

. A second integration on angular rate {ea*} (actually, integrands dominated by angular rate) produce 

changes in a , P and jd . The outer loop assumes that the commanded CMD can ^ u i c ^ly realized by 

in order to drive a , P and jd to their commanded values. 


Here, the faster inner-loop control of results directly from equations of motion governing the vehicle’s 

angular acceleration 



7 { b ^L + W h x/ M a 



(31) 


Above, the angular momentum {//^|^ about the vehicle’s center of mass satisfies ^ = I {co 1 ^ ^ where /, 

approximated as a constant, is the inertia matrix defined about the mass center. M T , M A and M Sk denote the 
external moments about the vehicle’s mass center due to thrust, baseline aerodynamics, i.e. with zero control 
deflections, S k , and the aerodynamic increment of each effector S k . Independent parameters for the moments 
include angle of attack, side-slip angle, dynamic pressure, effector position, and center of mass locations. 


Dynamic inversion can 



— {M t + M A } B . 


( 32 ) 
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A stabilizing control defines as =K.(|^|^ CMD _ {^s} g ) where 

K ? . = diag ( T k pb k qb k rb 1 ). Assuming there is sufficient control power, or there exists a Sk that satisfies 

k 

equation (32), the closed-loop response for this faster inner-loop is 

kpb kqb k rb ^ 

s + k pb s + k qb s + k rb \ 

Limits on control power include position and rate limits for the vehicle’s various effectors and are dealt with in the 
next section on control allocation. 



(s) = diag(\ 


V. Control Allocation 

Since the inner-loop control and control allocation problem deal solely with vectors in body axis coordinates, the 
bracket designation is dropped and (32) is expressed as 



Y J M Sk (q°,a 0 ,p°,8 k ) + £ M Sk {q\a\p\8 l ,8 j )=M s _ d 

k = 1 j =t h + 1 


(34) 


where 1 <l<n x , q denotes dynamic pressure , and M s des denotes the target moment corresponding to the right 
side of equation (32) and superscript ( 0 ) denotes the current value. The problem is to find 8 k that satisfies (34), 
subject to rate and position limits, with the slower varying q,a,p parameters set to their current values. The 

control moment increments considered are one of two types: One is a function of a single effector deflection and the 
other is a function of two, modeling the interference effects of one effector on another. For this development, 
assume that the current values of q,a,p, and S k are available. Expand (34) as a Taylor Series approximation about 
the current value as 


£ M «+2X A<y * + 2 ] b °sk AS k ~ M 5-des wh ere 


(35) 


k=n l +l 


r)M "3 f)M 

b 0 ^ = —^(q° ,a° , ,S k ) + ^-—g-(q° ,a° , ,8°) for 1 <k<n l ,j>r h 


dS„ 


j=i d8 k 


b° Sk =^L(q 0 ,a 0 ,j3°,8°,8 k ) for \<l<r\, k>r\, and 
dS h 



Y j M Sk (c l \a\p\8t)+ X M Sk (q 0 ,a°,/? 0 ,8° k ,8^. 


k=n l +l 


(36) 


With this approximation, the control allocation problem can be expressed on an incremental level, with simpler 
notation, as one of finding some optimal A u - |^A S l A S 2 • • • A 8 n§ J satisfying 


B°Au = m des = M s 


des 



k 


( 37 ) 
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where B° = \j? 5X b 52 • • • b Srig J , subject to all the limitations of rate and position on the elements of A u . The 

resulting control is defined as u - u° + Au . 

In [3], the standard minimum norm solution of (37), that essentially minimizes control rate, is replaced by one that 
minimizes both control rate and position. The quadratic cost function is 

/ = .5 (Au/Atf W rr (Au/At) + .5 (u° + Au) W pp (u° + Au) (38) 


where W rr and W pp are rate and position diagonal weighting matrices, respectively. To minimize (38) subject to 
(37), define the Lagrangian equation 

L = .5Au T W r Au+.5(u° + Au) W pp («° + Au) + X r ( B°Au - m Des ) (39) 

where W r = W rr / At 2 . The necessary condition for optimal Au [6] yields 


Au = W~ l B 0T ( B°W~ l B 0T )"’ m des - 

I nS -W~'B ot {B°W-'B° T y B° W~ l W p y 

= K*m^-[l n s-B° w *B°]w- l W p y 


(40) 


where W = W r + W pp and B™ = W l B 0T ^B°W 1 B ot ) . If W pp = 0 , (40) yields the minimum norm solution. The 

first term in (40) satisfies (37). The second term in (40) is in the null space of col |^° | and provides the correction 

II 0 II 2 
to reduce \\u +A u\\ 

\\w 

pp 

Although (40) minimizes effector rate and position activity, tending to zero counterbalanced effectors, it does not 
guarantee the commanded controls will be within their saturation limits. A multi-pass strategy is added to the 
allocator solution to reduce violations of limits. 

To satisfy both position and rate constraints of the effectors, the incremental control is constrained prior to 
commanding the hardware as 


u i,Y im - u o^ Au ^ U u,y im ~ u o 


\lun At ^ Au ^K,lun At ( 41 ) 

where [u llim ,u u]im ) and {u ilim ,u u]im ) are respectively the lower and upper position and rate limits of the effectors 
applied to the control command u cmd - u° + Au . Let k = 1 correspond to the first pass solution given by (40), i.e., 

A u^ = Au , and assume that at least one control effector command A violates a saturation limit. For any k > 1 , 
define Au\^ e R nS as follows. If A uf^ , i - l,n s , violates one of the constraints in (41), denoted as Au. , set 
A u^\ = A u i . Otherwise, set A u[*\ =0. 

The portion of m des achieved by the constrained effectors is 
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(42) 


„« - , 


= B A «L • 


,(*) 


For the (A+l )-pass solution, solve first for A u 


(t+i) 


B°Au { 0 k+1) = • 


- m 


..(*) 

des,\\m 


: 

J des 


(43) 


using (40) with /j^ replacing and the diagonal elements of W~ l corresponding to constrained elements ( all i 
such that Au^. ^ 0. ) set to zero. From (40), only the unconstrained controls are used in the solution: elements of 
A u^ +1 ^ corresponding to constrained effectors are zero. The incremental control solution for the (k + 1) -pass is 

A u {k+l) = Au^ +1) +Au^. (44) 

If A i/ k+])> satisfies (41), set A u - A i/ k+1 ^ . Otherwise, set k - k + 1 and repeat. To avoid driving all controls into 
saturation when excessive control power is required, limit k< 2 . Note that the first pass corresponds to Aw{^ ; = 0 
for all i ,n s . 

Implementation of the control allocator requires tables describing both force and moment coefficients generated by 
the baseline aerodynamics and the increments due to effector position. Furthermore, tables are required of the 
partial derivatives of the force and moment increments to effector position. In the simulation, the aero database is 
linearly interpolated across all independent parameters. The simplest approach to constructing the partial derivative 
tables is to double the effector's intermediate breakpoints S k — » S h k l ~s,S h k l +s , providing the required support to 
the constant slope between the original breakpoints indicative of linear interpolation. All the tabular data express 
the moment about a specific aerodynamic reference, so these must typically be translated to vehicle's center of mass 
using the force data to create M° Sk and B° in (37). 

It is worth noting that the control allocation algorithm produces an approximate solution to (34), even if the 
simulation uses linear interpolation. Over the multi-pass iterations, B° does not change. As a result, the solution 
generated by the control allocator in the simulation is exact only when no control solution passes a table breakpoint 
due to the change in slope —even if no control violates its saturation limits. Since many effectors have different 
slopes on either side of zero deflection, differences can be expected in this neighborhood of small effector 
deflections. Only if the effectors effectiveness are strictly linear (not piecewise) will there be no control allocation 
error. 


VI. Guidance 

In figure 3, the closed-loop guidance law is responsible for providing the command q N2C = q M *q_ p *q a and a 
thrust command T c required to follow a desired trajectory. The actual trajectory is specified by both the speed, 
V = (the origin of N is attached to vehicle's center of mass) and the orientation of the N with respect to I 

specified by q I2N =q z *q r as a function of time. The desired trajectory is defined by time histories of the 
commanded q I2G = q z *q r and V CMD = \v'. . 

To obtain the desired guidance outputs, consider the point-mass equation of motion expressed in the A^-frame 
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M { / ^l= M ({^L + Kl x WL)={^L + {^L + { F G }. 


(45) 


where F A , F T , and F G respectively denote the aerodynamic, thrust, and gravitational force, and M is the vehicle 
mass. Let the aerodynamic forces be expressed in the wind-axis frame W in terms of lift (L), drag (£>), and side 
force (Y) as = ~F>w x + Yw y -Lw z . Wind axis bank angle orients W from N as q N2W = q M requiring q~ ] to 

transform {F a } w to {F a }^ . The thrust force is generally expressed in body axis as = T x b x + T y b y + T z b z 

requiring q N \ B to transform it to frame N. Assuming flat earth, the gravitational force is expressed in the inertial 
frame as {F g } / = Mgi z and transformed to frame N with q J2N . 

Transforming the forces to the N frame (using direction cosine matrices to avoid manipulating half-angle sines and 
cosines [5]) and noting j = Vn x , (45) can be expressed in component form as 

MV = -D + T x cos(/?)cos(or) + r sin (/?) + T cos (/?) sin (a) -Mg sin (/) 

MV = Y cos(//) + Lsin(//) 

+T x (sin (//) sin (a) - cos (//) sin (/?) cos (or)) 

+T y (cos (ju) cos (/!)) 

—T z (sin (//) cos («) + cos (//) sin (/?) sin (#)) 

-MV = Y sin(//)-Lcos(//) + Mgcos(;r) 

—T x (cos (//) sin (cir) + sin (//) sin (/?) cos (or)) 

+^(sin(//)cos(/?)) 

+T z (cos ( ju) cos (cir) - sin (//) sin (/?) sin (#)) 

Assume that Y , T y and T z are zero and that the control design will keep j3 close to zero so that (46-48) simplify to 

MV = -£> + T cos (a) - Mg sin (/) (49) 

MV {cob = Lsin(//) + r (sin(//)sin(a)) (50) 

-MV {calf = -Lcos (//) + Mg cos (/)-T (cos(//)sin(a)) (51) 

Required T x , a and ju commands will be obtained by inverting equations (49-51) to satisfy desired values V des , 
t and {cob ^ designed to drive the actual trajectory to the desired one. The desired V des will be 
defined as 

V des =K(V CMD -V). (52) 
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The desired angular rate will be obtained as J from a quaternion-based control directly taken from the 

simple example utilizing q I2N and the commanded q I2G . Assume for the inversion, that the slower varying 
parameters V and y are set to their current values, V = V° and y = y°. 


To obtain /u c , subtract the gravity term in (51) from both sides of the equation and divide the result into (50) to yield 


tan {Me) 


MV ° 


(53) 


Here it is assumed that V° and cos(/° ) are not both zero. [4] 

To obtain the thrust command, T c , and a c , first multiply (50) by sin(//) and subtract from it (51) multiplied by 
cos(//) to yield 

L(y 0 ,a c ,S° k ) + T c sin (a c ) = MV' 3 {«£ } w ^ sin ( Mc ) 

, v (54) 

+M \V° { <} N y des +gcos(y°)}cos( Mc ) 

Equation (54) shows the dependency of lift on angle of attack, airspeed (through dynamic pressure) and effector 
position. Lift is also dependent on side-slip which is assumed to be commanded to zero. Equation (54) along with 
(49), or 

D(v°,a c ,S° k )-T c cos (a ) = ~MV des - Mg sin ( / ) (55) 


provide the two equations to be solved for T c and a c . Due to the nonlinear dependency of L and D on a c an 
iterative procedure is used to solve for T c and a c . Since ju c is set by (53) and the current values y° and V° 
remain constant over the iterative process, the following shorthand for (54-55) is adopted. 

h (<*c Jc ) = L(v\ a c , $ ) + T c sin (a c ) = C lift 

f D (a c ,T c ) = D(v°,a c ,S° k )-T c cos (a c ) = C dmg (56) 


Here, an iterative Newton method [7], based on the Taylor series expansion of the (56) at current iterative values 
a c (k) and T c (k) , is used to update the command as a c [k + 1) = a c ( k ) + s a Aa c (/:) and 

T c [k + l) = T c (k) + s T AT c (k) where s a and s T control the step size (<1) with the correction satisfying the linear 
equations 


da 


A a + 


%\aJ) 

dT 


'a,<k) 

T c (k) 


AT = C m 


f L (a c (k),T c {k)) 


(57) 


dfp(a,T) l 

da 


A a + 


djjMJ) 

dT 


^a c (k) 
T c (k ) 


AT = C, 


drag 


-f D { a c{ k )’ T c( k )) 


(58) 
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The iteration begins by setting a c (0) = a° and T c (0) = T° . 

Implementation requires additional tables for lift and drag coefficients and their partial derivatives with respect to 
angle of attack. Generally aerodynamic force coefficients are given with respect to the body axis, and these must be 
converted to the wind axis for use in the guidance algorithm, i.e. 

C D = -cos(/?)cos(or)C x -sin(/?)C F -cos(/?)sin(a)C z (59) 

C L =sin(a)C x -cos {a)C z (60) 

Since linear interpolation is used in the simulation, the partial derivative tables are constructed consistently with 
those for the control derivatives in the control allocation algorithm. Here, however, there are many more break 
points for angle of attack than for the effector position. In closing, it should be mentioned that the Newton method 
may not be the best way to solve (56). The rough approximation of the Newton method restricted the process to two 
iterations where s a - .1 and s T - .4 to prevent destabilizing the guidance loop. Evidently the linear dependency on 
thrust in (56) allows a larger correction for T c . 

Having described the quaternion-based control, an example is presented in the next section. To be noted, the two 
PID controllers for guidance and control use six gains, the inner loop controlling body rate j uses three gains, 

and the definition of V des uses one for a total of ten gains to be selected. 

VII. Example 

To demonstrate both the guidance and control aspects of the algorithm, the subscale GTM vehicle [8] with 17 
effectors is flown first in simulation with the guidance loop open. Commands for angle of attack , wind-axis bank 
angle and sideslip angle are direct inputs to the a - /3 - ju outer-loop control. A flight path trajectory is obtained 

from the resulting maneuver and then used to fly the system with the guidance system engaged. At this stage in the 
tool's development, the guidance law is set to follow a known-reachable trajectory. For our purposes, the vehicle is 
trimmed in a level turn to the pilot's right with the vehicle banked 40 degrees right wing down (trimmed a - 4.34 , 
V eas - 95 knots). At 5 seconds, the pilot pulls up one degree of angle of attack and at 10 seconds reverses the bank 
angle to -40 degrees producing a S- shaped maneuver. The throttle setting is unchanged, although thrust changes due 
to changing flight conditions. 

This flight path generation maneuver provides an initial check on the performance of the outer loop a- (3- ju 

control, the inner-loop control of , and the control allocator. The GTM has an expanded suite of 17 effectors 

resulting from split elevator, spoilers, and flaps, each with four surfaces designated as right/left and 
inboard/outboard. The rudder is split into upper and lower surfaces, but the right and left ailerons are not split. The 
stabilizer has also been actuated, though this capability does not exist in the flight test vehicle, to exercise the 
allocator ability to handle surfaces that influence one another. In this case it is the effectiveness of the elevator that 
is changed due to the position of the stabilizer. The actuators driving these surfaces are all modeled as first-order 
systems with 5 Hz bandwidth and a rate limit of 300 deg/sec. The position limits vary for the surfaces and are given 
in table 1 . The GTM example includes effectors that can be deployed positively or negatively as well as some 
(spoilers and flaps) that do not have negative deflections, providing a good test of the proposed allocation algorithm. 
All aerodynamics associated with the baseline and the surface increments were obtained from wind tunnel testing, 
with the exception of the flaps, which were modeled with a constant control derivative. 
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The tracking results for the outer loop a- p- ju control are shown in figure 5. The largest overshoots occur in 
response to most aggressive portion of the maneuver, the 80 degree change in wind-axis bank angle. But the 
overshoot of ju is less than two degrees. Side-slip angle is maintained close to zero, while the angle of attack is 

slightly perturbed from its one degree increase during the roll. The inner-loop tracking results for body angular rate 
are shown in figure 6. The largest overshoot occurs in response to the commanded roll rate: the vehicle's dynamics 
will not follow transients of that magnitude. The commanded spike is, however, relatively well damped as a result 
of the PID gains selected. 


Effector 

Designation 

Position Limit 
(deg) 

Rate Limit 
(deg/sec) 

Aileron 

L, R 

[- 20 , 20 ] 

300 

Stabilizer 


[- 12 , 4 ] 

300 

Elevator 

LOB, LIB, RIB, ROB 

[- 30 , 20 ] 

300 

Rudder 

U,L 

[- 30 , 30 ] 

300 

Spoiler 

LOB, LIB, RIB, ROB 

[ 0,[45 15 15 45 ]] 

300 

Flap 

LOB, LIB, RIB, ROB 

[ 0 , 30 ] 

300 


Table 1. Effector Position and Rate Limits 

To check the performance of the control allocator, the desired moment change required of the effector k is defined as 
b k AS k where b\ is the k column of B° and A S k is the commanded incremental change in current effector position 

S k . Assuming that A S k does not violate any limit on the last pass of the control allocator iterative algorithm, 
b k AS k is the desired change in moment from the k effector. In the example, the linear solution produced the 
requested moment. To determine how well the control derivative reflected the actual effector control effectiveness, 
consider the error 


M Se (a 0 ,j3\S 0 k ,AS k ) = b 0 k AS k -(M St (a 0 ,j3\S 0 k +AS k )-M Sk (a 0 ,j3 0 ,S° k )). 


(61) 


For this maneuver, the largest error was the pitching moment due to the right aileron shown in figure 7. Note that as 
the right aileron crossed the breakpoint at S ail r - 0 the slope changed enough to cause a four lbf-ft error in pitching 

moment. As expected there is no error for the linear flap model in figure 8. 

One concern regarding the allocator is illustrated in figure 8. The surfaces tend to end the maneuver at some non- 
zero deflection with no apparent tendency to retract. This not only occurred for the flaps, but the spoilers as well. 
The algorithm is formulated to reduce the deflections of counter-balanced effectors, but the simulation result does 
not indicate success in this example. Evidently more work needs to be done on the allocator method. 

To test the guidance law, airspeed, heading and flight path angle data from the previous run are now used as 
guidance commands with the guidance loop closed. Recall, the solution involved satisfying a the point mass 
equation of motion expressed in the N frame to generate , a c and T c . The guided results in figure 9 reveal good 
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tracking of airspeed, heading and flight path angle with airspeed error within ±0.2 knots and heading and flight path 
angle error within ±0.25 deg. The commanded a c and ju c in figure 10 have roughly the same character as the 
unguided results, but there is definitely more activity due to the guidance-generated a c . The outer loop a- ft- ju 
control still follows the commanded signals. Interestingly the desired used in point mass equation is 

realized by the actual N frame in figure 11. Not shown, the effector deployment well approximates that of the 
unguided result. 


Angle of Attack: a cmd , a 



Sideslip Angle: p cmd , p 



Wind Axis Bank Angle: p cmd , ju 



time (sec) 


Figure 5. Maneuver generating response 
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Roll Rate: p p 

'cmd 1 




Figure 6. Inner loop tracking 


20 

American Institute of Aeronautics and Astronautics 


dot (deg/sec) 


Ailerons: Position (Left, Right) 




Figure 7. Outer loop control: ailerons 
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Outboard Flaps: N Mismatch 



Figure 8. Outer loop control: outboard flaps 
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Figure 9. Guided results: velocity vector tracking 
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Angle of Attack: a a 

a cmd 



Wind Axis Bank Angle: |u cmd , ju 



time (sec) 


Figure 10. Guided results: angle of attack, sideslip, and wind axis bank angle 
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Figure 11. Guided results: angular rate of the N-frame with respect to I-frame 
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Figure 12. Guided results: commanded thrust and equation error in guidance solution 

There probably is, however, a better method for satisfying the point mass equation of motion. The present guidance 
law tends to destabilize the system if the number of iterations between control sample time is three or greater. 
Another deficiency can be seen in the equation error of the drag equation in figure 12. It dominates the mean 
squared error of the two equations (54-55) and causes undesirable oscillatory thrusting commands during the periods 
of commanded angle of attack changes. The one degree change in angle of attack produced a drag equation error 
spike of -15 lbf at 5 seconds. Perhaps one cause could be that the dynamic derivative models, producing moments 
due to angular body rates, were neglected in the build up for L and D. 

VIII. Concluding Remarks 

This paper has described and shown the potential of a quaternion-based aircraft control architecture for following 
candidate trajectories in simulation. The use of kinematic inversion coupled with the quaternion PID control was 
shown to produce the same level of tracking in all attitude angles. With the guidance loop open, the outer loop 
a- p- ju provided excellent tracking of a c , /? and ju c , when the effectors did not violate their position and rate 

limits. The maneuver used did not demonstrate the potential of the control allocation to address the problem 
effectors in saturation. The control allocation also failed to demonstrate that it would retract counterbalanced 
controls in the example given. Instead, the allocation algorithm ended the maneuver with all surfaces deployed — 
perhaps indicating an error in the implementation. The quaternion-based guidance showed good tracking of the 
desired velocity vector. Moreover, the commanded wind axis bank angle resulting from the guidance law appeared 
to duplicate the unguided command. More work, however, is needed to address the equation error problem during 
angular body rate changes in defining acceptable thrust and angle of attack commands. While the example 
demonstrated the potential of the proposed architecture to follow a commanded trajectory, it remains to be seen 
whether this architecture can be used define acceptable candidate trajectories based on the control power limitations 
of the vehicle. 
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